#include "IRlocate.h"
#define NUM_BALLS 2
#define MAX_BALLS 3

IRlocate::IRlocate()
{
	Requires (irsensors);
	Requires (lifter);
	Requires (conveyer);
}
void IRlocate::Execute()
{
	//irsensors->GetConveyerSensor();
	/*
	 * if( last lifter sensor !=1  && NUMBALLS< MAXBALLS)
	 * 		CommandBase::conveyer->Forward();
	 * else if (last lifter sensor ==1 && first lifter sensor ==0 && middle conveyer sensor ==1)
	 * 		CommandBase::conveyer->Stop();
	 * 
	 * if(last conveyer==1 && first lift sensor == 0 && !last lifter sensor ==1)
	 * 		CommandBase::conveyer->Forward();
	 * 		CommandBase::lifter->Forward();
	 * if(first lift sensor ==1 && !last lifter sensor==1)
	 * 		lifter->forward
	 
	if(!(irsensors->GetLifterSensor(1) && irsensors->GetLifterSensor(2) && irsensors->GetLifterSensor(4)) && NUM_BALLS < MAX_BALLS)
		CommandBase::conveyer->Forward();
	else if ((irsensors->GetLifterSensor(5) && irsensors->GetLifterSensor(6)) && !(irsensors->GetConveyerSensor(22)))
		CommandBase::conveyer->Forward();
	if(!(irsensors->GetLifterSensor(1) && irsensors->GetLifterSensor(2) && irsensors->GetLifterSensor(3)) && !(irsensors->GetLifterSensor(22)) && irsensors->GetConveyerSensor(22))
	{
		CommandBase::conveyer->Forward();
		CommandBase::lifter->Forward();
	}
	if(irsensors->GetLifterSensor(22) && !irsensors->GetLifterSensor(22))
		CommandBase::lifter->Forward();
	*/
}

bool IRlocate::IsFinished()
{
	return 0;
}
void IRlocate::End()
{
	
}
void IRlocate::Interrupted()
{
	
}
